#include "dyn_interpolation_poly.h"

PolyInterpolation::PolyInterpolation(colvec _xValues, colvec _yValues, int _interPoints)
    : Interpolation(_xValues,_yValues,_interPoints)
{
    setError(0.0);
}

double PolyInterpolation::rawInterpolation(int _index, double x)
{
    // We prepare the placeholders.
    int nearestIndex = 0;
    colvec c(interPoints),d(interPoints);

    // We declare vectors that hold the subrange
    // (lowerBound..lowerBound+interPoints-1).
    colvec xVec = xValues.subvec(span(_index,_index+interPoints-1));
    colvec yVec = yValues.subvec(span(_index,_index+interPoints-1));

    // We compute the difference between x and x(0), at the lower bound.
    double delta = fabs(x-xVec(0));
    double deltaTemp;

    // With it, we can find the index for which we are closest
    // to the point we want to interpolate.
    for (int i=0;i<interPoints;i++)
    {
        if ((deltaTemp = fabs(x-xVec(i))) < delta)
        {
            nearestIndex = i;
            delta = deltaTemp;
        }

        // We initialize the tableau of c's and d's.
        c(i) = yVec(i);
        d(i) = yVec(i);
    }

    // We can now write the initial approximation for y.
    double y = yVec(nearestIndex--);

    // We now update the c's and d's for each of the column.
    for (int m=1;m<interPoints;m++)
    {
        for (int i=0;i<interPoints-m;i++)
        {
            // We define the variables in Eq. (3.2.5).
            double numeratorA = xVec(i)-x;
            double numeratorB = xVec(i+m)-x;
            double numeratorC = c(i+1)-d(i);

            double den;

            if ((den=numeratorA-numeratorB) == 0.0)
            {
                cout << "Polynomial interpolation has failed." << endl;
                throw("Polynomial interpolation error.");
            }

            den = numeratorC/den;

            // We update the c's and d's
            c(i) = numeratorA*den;
            d(i) = numeratorB*den;
        }
        // We now update the interpolation value.
        y += (error = (2*(nearestIndex+1) < (interPoints-m) ? c(nearestIndex+1) : d(nearestIndex--)));
    }
    return y;
}
